from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook
from kinematics import Vector, Quaternion, Transform
import graphics
Манипулятор кинематической схемы PUMA обладает шестью степенями подвижности.

Его можно условно разделить на сегменты, соединияюще между собой:

Длина этих сегментов определена в таблице:
| Пара | длина |
|---|---|
| основание - плечо | $l_0$ |
| плечо - локоть | $l_1$ |
| локоть - кисть | $l_2$ |
| кисть - фланец | $l_3$ |
def puma_chain(q, l, math_source=np):
base = Transform.identity()
shoulder = base + Transform(
Vector(0, 0, l[0]),
Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source) *
Quaternion.from_angle_axis(q[1], Vector(0, 1, 0), math_source)
)
elbow = shoulder + Transform(
Vector(0, 0, l[1]),
Quaternion.from_angle_axis(q[2], Vector(0, 1, 0), math_source)
)
wrist = elbow + Transform(
Vector(0, 0, l[2]),
Quaternion.from_angle_axis(q[3], Vector(0, 0, 1), math_source) *
Quaternion.from_angle_axis(q[4], Vector(0, 1, 0), math_source)
)
flange = wrist + Transform(
Vector(0, 0, l[3]),
Quaternion.from_angle_axis(q[5], Vector(0, 0, 1), math_source)
)
return [base, shoulder, elbow, wrist, flange]
Зададим изменение обобщенных координат:
def puma_q(t, total):
#omega = t / total * np.pi * 2
a = (t - total) / 2
omega = (-np.cos(3.14 / total * t) + 1.0)*5
return [
np.pi / 4 * np.cos(omega),
np.pi / 6* np.sin(omega),
np.pi / 2,
np.pi*3/4,
np.pi *np.sin(omega)/ 6,
0
]
Зададим длины звеньев:
puma_l = [1, 2, 1, 0.5]
puma_fig = plt.figure()
ax = puma_fig.add_subplot(projection="3d")
ax.set_xlim([-2, 2]); ax.set_ylim([-2, 2]); ax.set_zlim([0, 4])
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)
total = 100
def animate(frame):
chain = puma_chain(puma_q(frame, total), puma_l)
(x, y, z) = graphics.chain_to_points(chain)
lines.set_data_3d(x, y, z)
global r, g, b
r.remove(); g.remove(); b.remove()
r, g, b = graphics.axis(ax, chain[-1], 0.5)
animate(0)
fps = 25
puma_ani = animation.FuncAnimation(
puma_fig,
animate,
frames=total,
interval=1000.0/fps
)
HTML(puma_ani.to_jshtml())